# -*- encoding:UTF-8 -*-
from naoqi import ALProxy
import time
import threading
import cmath

IP = "192.168.1.103"
PORT = 9559



def WALKFREQUENCY():
    maxstepx = 0.08
    maxstepy = 0.1
    maxsteptheta = 0.2
    # maxstepfrequency = 0.6
    maxstepfrequency = 0.4
    stepheight = 0.02
    torsowx = 0.0
    torsowy = 0.0
    moveConfig = [["MaxStepX", maxstepx],
                  ["MaxStepY", maxstepy],
                  ["MaxStepTheta", maxsteptheta],
                  ["StepHeight", maxstepfrequency],
                  ["TorsoWx", torsowx],
                  ["TorsoWy", torsowy]]


def standUp():
    postureProxy.goToPosture("Stand", 0.5)
    pass


def stretchArms():
    # ttsProxy.say("伸展手臂")
    motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderRoll", 70.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderPitch", 73.4 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", 73.4 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RWristYaw", 100 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LWristYaw", -100 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 4.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -4.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RHand", 1.0, 0.2)
    motionProxy.setAngles("LHand", 1.0, 0.2)
    time.sleep(1)
    pass


def curlArms():
    # ttsProxy.say("弯曲手臂")
    motionProxy.setAngles("RShoulderPitch", 0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", 0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    # motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LShoulderRoll", 70.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 2.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -2.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    # motionProxy.setAngles("RWristYaw", 100 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LWristYaw", -100 * cmath.pi / 180, 0.2)
    # time.sleep(1)

    # motionProxy.setAngles("RElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    pass


def catchTheBall():
    motionProxy.setAngles("RShoulderPitch", -8.1 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", -8.1 * cmath.pi / 180, 0.2)
    time.sleep(1)

    # ttsProxy.say("抓取小球")
    motionProxy.setAngles("RElbowRoll", 15.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -15.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderRoll", 15 * cmath.pi / 180, 0.1)
    motionProxy.setAngles("LShoulderRoll", -15 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RHand", 0.3, 0.2)
    motionProxy.setAngles("LHand", 0.3, 0.2)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", -20 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", -20 * cmath.pi / 180, 0.2)
    time.sleep(5)
    pass


def backCurlArms():
    ttsProxy.say("收回手臂")
    motionProxy.setAngles("RShoulderPitch", -50 * cmath.pi / 180, 0.05)
    motionProxy.setAngles("LShoulderPitch", -50 * cmath.pi / 180, 0.05)
    time.sleep(3)

    RShoudlerRollTheta = 11.2
    LShoudlerRollTheta = -11.2
    RElbowRollTheta = 13.7
    LElbowRollTheta = -13.7
    for i in range(10):
        RShoudlerRollTheta = RShoudlerRollTheta - 0.3
        LShoudlerRollTheta = LShoudlerRollTheta + 0.3
        motionProxy.setAngles("RShoulderRoll", RShoudlerRollTheta * cmath.pi / 180, 0.05)
        motionProxy.setAngles("LShoulderRoll", LShoudlerRollTheta * cmath.pi / 180, 0.05)

        RElbowRollTheta = RElbowRollTheta + 3
        LElbowRollTheta = LElbowRollTheta - 3
        motionProxy.setAngles("RElbowRoll", RElbowRollTheta * cmath.pi / 180, 0.05)
        motionProxy.setAngles("LElbowRoll", LElbowRollTheta * cmath.pi / 180, 0.05)

    motionProxy.setAngles("RShoulderPitch", -18 * cmath.pi / 180, 0.05)
    motionProxy.setAngles("LShoulderPitch", -18 * cmath.pi / 180, 0.05)

    pass


def move():
    motionProxy.setMoveArmsEnabled(False, False)
    # motionProxy.wakeUp()
    # motionProxy.move(0.1, 0.0, 0.0, moveConfig)
    for i in range(10):
        motionProxy.move(0, 0.1, 0.0, moveConfig)
        time.sleep(1)


class turnAround(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)

    def run(self):
        motionProxy.setMoveArmsEnabled(False, False)
        initangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
        while True:
            motionProxy.move(0, 0, 0.1, moveConfig)
            nowtangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
            dis = (nowtangle - initangle) * 180
            print dis
            time.sleep(1)


def back():
    for i in range(5):
        motionProxy.move(-0.1, 0, 0, moveConfig)
        time.sleep(1)
    motionProxy.stopMove()
    pass
def Catch():
    global motionProxy
    global ttsProxy
    global postureProxy
    global moveConfig
    global memorypxy
    maxstepx = 0.02
    maxstepy = 0.03
    maxsteptheta = 0.2
    maxstepfrequency = 0.1
    maxstepheight = 0.02
    torsowx = 0.0
    torsowy = 0.0
    # 双手 后退
    moveConfig = [["MaxStepX", maxstepx],
                  ["MaxStepY", maxstepy],
                  ["MaxStepTheta", maxsteptheta],
                  ["StepHeight", maxstepheight],
                  ["MaxStepFrequency", maxstepfrequency],
                  ["TorsoWx", torsowx],
                  ["TorsoWy", torsowy]]

    motionProxy = ALProxy("ALMotion", IP, PORT)
    ttsProxy = ALProxy("ALTextToSpeech", IP, PORT)
    postureProxy = ALProxy("ALRobotPosture", IP, PORT)
    memorypxy = ALProxy("ALMemory", IP, PORT)
    motionProxy.wakeUp()
    # motionProxy.moveTo(0.1, 0.0, 0.0)
    # time.sleep(2)

    # 站直
    standUp()

    # 伸展手臂
    stretchArms()

    # 弯曲手臂
    curlArms()

    # 抓取小球
    catchTheBall()

    # 收回手臂
    # backCurlArms()

    # move()

    # 转圈
    turn = turnAround()
    turn.start()

    # 后退
    back()

if __name__ == '__main__':
    Catch()
